﻿program program_psr
use madx_ptc_module
use pointer_lattice
implicit none

    interface
       subroutine build_PSR(PSR,MIS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: PSR
         logical(lp) mis
       end subroutine build_PSR
    end interface

type(layout), pointer:: psr
type(damapspin) id,m,a_t,a_f,a_s,a_l,a_nl
real(dp) closed_orbit(6),coeff(4),THETA0,prec,n2(3),as(3,3),ang
type(probe) ray_closed
type(probe_8) ray
type(internal_state) state
type(normal_spin) normal
integer :: no1=0,np1=0 ,i,j,mf,je(lnv),ic
type(taylor) x2,average_floquet,average_xp,radii(2),invariant(2)
logical(lp) mis
TYPE(spinor_8) N_AXIS
TYPE(spinor) N0_AXIS
type(integration_node) t
type(fibre),pointer :: p
real w(8,8)
real(dp) n0(3),l0(3),m0(3)

prec=1.d-10

call ptc_ini

psr=>m_u%start

mis=.false.


call build_PSR(PSR,mis)


 state=nocavity0+spin0


no1=1
np1=0    !!!  No system parameters in the map

call init(state,no1,np1) ! PTC and FPP are properly initialized

call alloc(id,m,a_t,a_f,a_s,a_l,a_nl)  
call alloc(ray)
call alloc(radii)
call alloc(invariant)
call alloc(normal)
call alloc(x2,average_floquet,average_xp)
call alloc(N_AXIS)

N0_AXIS%x=0.d0

write(6,*) " This program will track a canonical transformation of the spin "
write(6,*) " De facto doing a 'Twiss' for the spin  "
write(6,*) "   A_1^-1 o PTC_12 o A_0 "
write(6,*) "   The map A_1^-1 is the tracked on zeroth order"
write(6,*) " Therefore since the A_i's are zeroth order "
write(6,*) " the residual map will be a SLICK type map around "
write(6,*) " "
write(6,*) " Entrance tracked N_0 axis : enter 2 or 3 "
read(5,*) ic
write(6,*) " Let PTC compute the perpendicular m0 and l0 "
write(6,*) "       yes -> t          no -> f    "
read(5,*) mis

if(ic==2) then
 call kanalnummer(mf,"result_n0_2.txt")
else
 call kanalnummer(mf,"result_n0_3.txt")
endif

 N0_AXIS%x(ic)=1
 ID=1

if(mis) then
 if(ic==2) then 
  ang=-pi/2
 else
  ang=0
 endif
 call find_a(N0_AXIS,id,ang,n0,m0,l0)
else
  if(ic==2) then 
   n0=(/0,1,0/)
   m0=(/1,0,0/)
   l0=(/0,0,-1/)
 else
   n0=(/0,0,1/)
   m0=(/1,0,0/)
   l0=(/0,1,0/)  
 endif

  do j=1,3
  id%s%s(j,2)=n0(j)
  enddo
  do j=1,3
  id%s%s(j,3)=m0(j)
  enddo
  do j=1,3
  id%s%s(j,1)=l0(j)
  enddo
endif

 do i=1,3
 do j=1,3
  as(i,j)=id%s%s(i,j)
 enddo
 enddo

closed_orbit=0.d0 ! initial orbit

RAY_CLOSED=closed_orbit

 ! ray= closed orbit + Canonical  transformation to (n_0,m_0,l_0)
 ray=RAY_CLOSED+id;  

CALL TRACK_PROBE(PSR,RAY,STATE,FIBRE1=1) ! Map is computed via the ray

m=ray

id=m.cut.2
id%m=1  ! just the spin part needs to be present

id=id**(-1) ! Removing the constant part of n_0

m=id*m  

Write(mf,*) "  "
write(mf,*) "   A_1^-1 o PTC_12 o A_0: "
Write(mf,*) "  "

if(mis) then
 WRITE(mf,*) "Vectors m0 and l0 computed by FPP "
else
 WRITE(mf,*) "Vectors m0 and l0 inputed by user "
endif

WRITE(mf,'(a7,3(1x,E15.8),a1)') " n0 = (",n0,")"
WRITE(mf,'(a7,3(1x,E15.8),a1)') " m0 = (",m0,")"
WRITE(mf,'(a7,3(1x,E15.8),a1)') " l0 = (",l0,")"

do i=1,6
do j=1,6
je=0
 je(j)=1
 w(i,j)=m%m%v(i).sub.je(1:6)
enddo
enddo

Write(mf,*)
Write(mf,*) " initial change of basis matrix "
 WRITE(mf,'(3(4x,a7,5x))') '  l0  ','  n0  ','  m0  '
do i=1,3 
 WRITE(mf,'(3(1x,E15.8))') as(i,1:3)
enddo

do i=1,6
 je=0
 je(i)=1
w(8,i)=m%s%s(1,2).sub.je
w(7,i)=m%s%s(3,2).sub.je
enddo

Write(mf,*)
Write(mf,*) " Slick Matrix in PTC units "
do i=1,8
 WRITE(mf,'(8(1x,E15.8))') w(i,1:8)
enddo

Write(mf,*)
Write(mf,*) " Full PTC map "
Write(mf,*)
call print(m,mf,prec)

close(mf)

write(6,*) "   "
write(6,*) " hit return to terminate program "
read(5,*) 
write(6,*) "   "
 


end program program_psr


!=================================================================
subroutine  build_PSR(PSR,mis)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: PSR

real(dp) :: ang, brho, kd, kf, Larc,SG(6),cut
type(fibre) :: b, d1, d2, qd, qf
type(layout) :: cell
logical(lp) mis
type(work) w
!-----------------------------------

call make_states(.true.)
exact_model = .false.
default = default + nocavity  
call update_states
madlength = .false.




!Dipole parameters:
!           S-bend, no fringe field !
!	         L −  2.0 m along the curved axis
!            θ − 106.8/54*π/180 rad = 0.0345187 rad

!For electrons at 5 GeV

ang = (twopi * 36.d0 / 360.d0)
Larc = 2.54948d0
brho = 1.2d0 * (Larc / ang)
call set_mad(kinetic = 5.d0, method = 6, step = 100)
madkind2 = kind7  !drift_kick_drift

kf =  2.72d0 / brho
kd = -1.92d0 / brho

d1 = drift("D1", 2.28646d0)
d2 = drift("D2", 0.45d0)
qf = quadrupole("QF", 0.5d0, kf)
qd = quadrupole("QD", 0.5d0, kd)
ang= 106.8d0/54.d0*pi/180;
larc=2.d0
b  = sbend("B", Larc, ang)
!write(6,*) b%mag%bn(1)
!call add(b,1,0,0.d0)
!write(6,*) b%mag%bn(1)
!pause 7
b%mag%p%kill_ent_fringe=.true.
b%mag%p%kill_exi_fringe=.true.
b%magp%p%kill_ent_fringe=.true.
b%magp%p%kill_exi_fringe=.true.
!cell = d1 + qd + d2 + b + d2 + qf + d1
b%charge=1
call add(b,1,0,b%mag%bn(1)*b%charge)
cell=1*b;   !+d1+qd;
!write(6,*) cell%start%charge
!pause 888
w=b
!write(6,*) " mass , beta",w%mass,w%beta0
!pause
PSR = 1 * cell
PSR = .ring.PSR

call survey(PSR)


!if(mis) then
! sig=1.d-5
! cut=4.d0
! call MESS_UP_ALIGNMENT(PSR,SIG,cut)
!endif
end subroutine build_PSR
